'''
Created on 23 mars 2012

@author: kwann
'''
import pygame
from pygame.locals import *
import roomba
import sensors

pygame.init()

screen = pygame.display.set_mode((640, 480))

roomba = roomba.Roomba('/dev/ttyUSB0', baud=19200)

roomba.start()
roomba.safe()
roomba.leds(0, 255)

#roomba.stream_samples(sensors.LEFT_ENCODER, sensors.RIGHT_ENCODER, sensors.DISTANCE, sensors.ANGLE)
print 'yes !'
motors = [0, 0]

def set_motor(motor, value):
    global motors
    motors[motor] = value

n = 0

while True:
    events = pygame.event.get()
    for event in events:
        if event.type == KEYDOWN:
            print 'keydown'
            if event.key == K_UP:
                set_motor(1, 255*0.3)
                set_motor(0, 255*0.3)    
                print 'up'
            elif event.key == K_DOWN:
                set_motor(1, -255*0.3)
                set_motor(0, -255*0.3)
                print 'down'
            elif event.key == K_SPACE:
                set_motor(0, 0)
                set_motor(1, 0)
                print 'space'
                roomba.leds(0, 255)
                roomba.close()
                pygame.quit()
                exit(0) 
    roomba.drive_pwm(*motors)
#       try:
#        print roomba.poll()
#    except Exception as e:
#        print e
#        break

roomba.close()